#include "msp430g2553.h" // Include the necessary header for our target MCU

#define MOTORA1_ON P2OUT |= BIT0
#define MOTORA2_ON P2OUT |= BIT1 
#define MOTORB1_ON P2OUT |= BIT2
#define MOTORB2_ON P2OUT |= BIT3 

#define MOTORA1_OFF P2OUT &= ~BIT0
#define MOTORA2_OFF P2OUT &= ~BIT1 
#define MOTORB1_OFF P2OUT &= ~BIT2
#define MOTORB2_OFF P2OUT &= ~BIT3 

#define RF_A (P1IN & BIT2)
#define RF_B (P1IN & BIT3)
#define RF_C (P1IN & BIT4)
#define RF_D (P1IN & BIT5)

void delay_ms(unsigned int ms ){ 		// Not really a milliseconds delay!
      unsigned int i;
      for (i = 0;i<= ms; i++){ 		// The operator is <=, I've yet to fix this site bug.
        __delay_cycles(500);
      }
}
 
void Forward()
{
	
	MOTORA1_ON;
	MOTORA2_OFF;
	MOTORB1_OFF;
	MOTORB2_ON;	
}

void Reverse()
{
	MOTORA1_OFF;
	MOTORA2_ON;
	MOTORB1_ON;
	MOTORB2_OFF;
}

void Right()
{
	MOTORA1_ON;
	MOTORA2_OFF;
	MOTORB1_ON;
	MOTORB2_OFF;
}

void Left()
{
	MOTORA1_OFF;
	MOTORA2_ON;
	MOTORB1_OFF;
	MOTORB2_ON;
}

void Stop()
{
	MOTORA1_OFF;
	MOTORA2_OFF;
	MOTORB1_OFF;
	MOTORB2_OFF;
}

void main(void) {
 
	WDTCTL = WDTPW + WDTHOLD;		// Hold the WDT (WatchDog Timer)
	P2DIR |= BIT0|BIT1|BIT2|BIT3;       		// Setup the appropriate output bits	
// 	P1DIR |= BIT7|BIT5|BIT6|BIT4; 

			Stop();
delay_ms(20);	
while(1) {
		 if( (!(RF_A))&&(!(RF_B)) ){				// Otherwise...
			Stop();
		}
		else if( (!(RF_D))&&(!(RF_C)) ){				// Otherwise...
			Stop();
		}
		else if ( !(RF_A) ){		// If not (port one input AND bit3 (sw1))
			Forward();
		}
		else if( !(RF_B) ){				// Otherwise...
			Reverse();
		}
		else if( !(RF_C) ){				// Otherwise...
			Right();
		}
		else if( !(RF_D) ){				// Otherwise...
			Left();
		}
 //}

 delay_ms(500);
	}
 
}
